#include "PathFinder.h"
#include "api/GameInfo.h"
#include "Utils.h"

#include <cassert>

///////////////////////////////////////////////////////////////////////////////////////////////////
PathFinder::PathFinder(const LevelInfo* level)
    : m_Width(level->width)
    , m_Height(level->height)
    , m_Level(level)
{
    m_Data = new uchar[m_Width * m_Height];
    m_Nodes = new Node[m_Width * m_Height];

    for (uint x = 0; x < m_Width; ++x)
    {
        for (uint y = 0; y < m_Height; ++y)
        {
            m_Data[x + y * m_Width] = (uchar) ((m_Level->blockHeights[x + y * m_Width] > 0.f) ? BLOCKED : WALKABLE);
        }
    }
}

///////////////////////////////////////////////////////////////////////////////////////////////////
bool PathFinder::FindPath(const Vector2& start, const Vector2& dest, Path& res, bool smooth, bool include_start)
{
    vector<uint> open_set;
    vector<uint> neighbors;

    InitData();

    Node& start_node = m_Nodes[GetIndex(start)];
    start_node.index = GetIndex(start);
    start_node.g_score = 0.f;
    start_node.f_score = HeuristicCostEstimate(start, dest);
    start_node.flag = OPEN;
    open_set.push_back(start_node.index);

    while (!open_set.empty())
    {
        Node& current = FindNodeWithLowestScore(open_set);

        if (current.index == GetIndex(dest))
        {
            ReconstructPath(res, start_node.index, current.index, smooth);
            if (include_start)
                res.insert(res.begin(), start);
            res.push_back(dest);
            return true;
        }

        open_set.erase(std::find(open_set.begin(), open_set.end(), current.index));
        current.flag = CLOSED;

        neighbors.clear();
        //GetNeighbors4(current.index, neighbors);
        GetNeighbors8(current.index, neighbors);

        for (uint i = 0; i < neighbors.size(); ++i)
        {
            Node& neighbor = m_Nodes[neighbors[i]];
            if (neighbor.flag == CLOSED)
                continue;

            float tentative_g_score = current.g_score + GetPosition(neighbor.index).distance(GetPosition(current.index));

            bool neighbor_not_in_open_set = std::find(open_set.begin(), open_set.end(), neighbor.index) == open_set.end();

            if (neighbor_not_in_open_set)
            {
                open_set.push_back(neighbor.index);
                neighbor.g_score = tentative_g_score;
                neighbor.f_score = neighbor.g_score + HeuristicCostEstimate(GetPosition(neighbor.index), dest);
                neighbor.come_from = current.index;
            }
            else if (tentative_g_score < neighbor.g_score)
            {
                neighbor.g_score = tentative_g_score;
                neighbor.f_score = neighbor.g_score + HeuristicCostEstimate(GetPosition(neighbor.index), dest);
                neighbor.come_from = current.index;
            }
        }
    }
    return false;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
void PathFinder::InitData()
{
    for (uint i = 0; i < m_Width * m_Height; ++i)
    {
        m_Nodes[i].index = i;
        m_Nodes[i].g_score = -1.f;
        m_Nodes[i].f_score = -1.f;
        m_Nodes[i].flag = NOT_SET;
    }
}

///////////////////////////////////////////////////////////////////////////////////////////////////
float PathFinder::HeuristicCostEstimate(const Vector2& start, const Vector2& end)
{
    return start.distance(end);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
PathFinder::Node& PathFinder::FindNodeWithLowestScore(vector<uint>& set)
{
    uint ind = 0;
    float smallest_f = m_Nodes[set[0]].f_score;
    for (uint i = 0; i < set.size(); ++i)
    {
        if (m_Nodes[set[i]].f_score < smallest_f)
        {
            smallest_f = m_Nodes[set[i]].f_score;
            ind = i;
        }
    }
    return m_Nodes[set[ind]];
}

///////////////////////////////////////////////////////////////////////////////////////////////////
void PathFinder::GetNeighbors4(uint index, vector<uint>& neighbors)
{
    Vector2 pos = GetPosition(index);
    uint x = (uint) pos.x;
    uint y = (uint) pos.y;

    if (x > 0)
    {
        index = x - 1 + y * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x < m_Width - 1)
    {
        index = x + 1 + y * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (y > 0)
    {
        index = x + (y - 1) * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (y < m_Height - 1)
    {
        index = x + (y + 1) * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }
}

///////////////////////////////////////////////////////////////////////////////////////////////////
void PathFinder::GetNeighbors8(uint index, vector<uint>& neighbors)
{
    Vector2 pos = GetPosition(index);
    uint x = (uint) pos.x;
    uint y = (uint) pos.y;

    if (x > 0)
    {
        index = x - 1 + y * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x < m_Width - 1)
    {
        index = x + 1 + y * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (y > 0)
    {
        index = x + (y - 1) * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (y < m_Height - 1)
    {
        index = x + (y + 1) * m_Width;
        assert(index < m_Width * m_Height);
        if (m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x > 0 && y > 0)
    {
        index = (x - 1) + (y - 1) * m_Width;
        assert(index < m_Width * m_Height);

        if (m_Data[x - 1 + y * m_Width] == WALKABLE && m_Data[x + (y - 1) * m_Width] == WALKABLE && m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x > 0 && y < m_Height - 1)
    {
        index = (x - 1) + (y + 1) * m_Width;
        assert(index < m_Width * m_Height);

        if (m_Data[x - 1 + y * m_Width] == WALKABLE && m_Data[x + (y + 1) * m_Width] == WALKABLE && m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x < m_Width - 1 && y > 0)
    {
        index = (x + 1) + (y - 1) * m_Width;
        assert(index < m_Width * m_Height);

        if (m_Data[x + 1 + y * m_Width] == WALKABLE && m_Data[x + (y - 1) * m_Width] == WALKABLE && m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

    if (x < m_Width - 1 && y < m_Height - 1)
    {
        index = (x + 1) + (y + 1) * m_Width;
        assert(index < m_Width * m_Height);

        if (m_Data[x + 1 + y * m_Width] == WALKABLE && m_Data[x + (y + 1) * m_Width] == WALKABLE && m_Data[index] == WALKABLE)
            neighbors.push_back(index);
    }

}

///////////////////////////////////////////////////////////////////////////////////////////////////
void PathFinder::ReconstructPath(Path& path, uint start_index, uint end_index, bool smooth)
{
    uint i = end_index;
    Vector2 pos;

    if (smooth)
    {
        uint prev_i = end_index;
        Vector2 last_pos;

        while (m_Nodes[i].come_from != 0xFFFFFFFF && i != start_index)
        {
            pos = GetPosition(i);

            if (!path.empty())
            {
                last_pos = path.back();
                if (Utils::IsLOS(m_Level, last_pos, pos, 0.f))
                {
                    prev_i = i;
                    i = m_Nodes[i].come_from;
                    continue;
                }
            }

            pos = GetPosition(prev_i);
            pos.x += 0.5f;
            pos.y += 0.5f;
            path.push_back(pos);
        }
    }
    else
    {
        while (m_Nodes[i].come_from != 0xFFFFFFFF && i != start_index)
        {
            i = m_Nodes[i].come_from;
            pos = GetPosition(i);
            pos.x += 0.5f;
            pos.y += 0.5f;
            path.push_back(pos);
        }
    }

    // reverse
    Vector2 tmp;
    uint size = path.size() - 1;
    for (uint i = 0; i < size / 2; ++i)
    {
        tmp = path[i];
        path[i] = path[size - i];
        path[size - i] = tmp;
    }
}

///////////////////////////////////////////////////////////////////////////////////////////////////
void PathFinder::SmoothPath(const Path& path, Path& res_path)
{
    uint prev_i = 0;
    res_path.push_back(path[0]);

    for (int i = 1; i < path.size(); ++i)
    {
        Vector2 last_pos = res_path.back();
        if (Utils::IsLOS(m_Level, last_pos, path[i], 0.f))
        {
            prev_i = i;
            continue;
        }

        res_path.push_back(path[prev_i]);
    }

    if (res_path.back() != path.back())
        res_path.push_back(path.back());
}